#include "path.h"

/**
 * \brief 成员函数定义
 * \name PATH
 */
void path::generate_head()
{
  // 获取当前位置并生成路径点
  vpoints.emplace_back(car_cur.x, car_cur.y, 1);
  vpoints.emplace_back(car_cur.x, car_cur.y + 0.4, 1);
  vpoints.emplace_back(car_cur.x / 2, car_cur.y + 0.7, 1);
  vpoints.emplace_back(0.2, -0.7, 1);
  vpoints.emplace_back(0.2, -0.21, 1);
}

void path::generate_back()
{
  // 获取当前位置并生成路径点
  vpoints.emplace_back(car_cur.x, car_cur.y, 1);
  vpoints.emplace_back(car_cur.x, car_cur.y + 0.3, 1);
  vpoints.emplace_back(car_cur.x / 2, car_cur.y + 0.6, 1);
  vpoints.emplace_back(0.05, -0.7, 1);
  vpoints.emplace_back(0.05, -0.21, 1);
}

void path::generate_side()
{
  // 获取当前位置并生成路径点
  vpoints.emplace_back(car_cur.x, car_cur.y, 1);
  vpoints.emplace_back(car_cur.x, car_cur.y + 0.2, 1);
  vpoints.emplace_back(car_cur.x / 2, car_cur.y + 0.6, 1);
  vpoints.emplace_back(0.15, -0.7, 1);
  vpoints.emplace_back(0.08, -0.3, 1);
}

int path::generate(my_msg::LaserScans* msg)
{
  if ( get_position(msg) == 0 ) {
    return 0;
  }
  switch ( type ) {
  case HEAD:
    generate_head();
    break;
  case BACK:
    generate_back();
    break;
  case SIDE:
    generate_side();
    break;
  default:
    cout << "\033[31m\033[45mERROR: path type is wrong!"
         << "\033[0m" << endl;
  }
  generate_vpoints(10);
  for ( auto point : vpoints ) {
    cout << point.x << ",";
  }
  cout << endl;
  cout << endl;
  cout << endl;
  cout << endl;
  for ( auto point : vpoints ) {
    cout << point.y << ",";
  }
  generate_dvp();

  cout << "generate is done !" << endl;
}

/**
 * @param msg
 * @return 1 表示进行追踪算法\n
 *         0 表示重新进行定位
 */
int path::count_path(my_msg::LaserScans* msg)
{
  // 停车完成
  if ( flag == 2 ) {
    cout << "\033[31m\033[44mstop node is done!!!\033[0m" << endl;
    return 0;
  }
  // 获取当前位置
  if ( get_position(msg) == 0 ) {
    return 0;
  }

  double t;
  if ( type == SIDE ) {
    // 这里是侧方停车决策的地方
  }
  else if ( type == HEAD || type == BACK ) {
    if ( flag == 0 ) {
      cout << "trace_two_sign" << endl;
      t = trace_two_sign();
    }
    else if ( flag == 1 ) {
      cout << "trace_one_sign" << endl;
      t = trace_one_sign();
    }

    if ( type == HEAD ) {
      return 10 * (int)(t * 180 / Pi);
    }
    else {
      return -10 * (int)(t * 180 / Pi);
    }
  }
  return 0;
}

int path::get_position(my_msg::LaserScans* msg)
{
  vector<line> lines;
  int          len = msg->ranges.size();
  line*        t   = new line();
  /**************************************************************************
     * 将线段识别并添加进lines
     * 逻辑有点复杂，懒得写注释了
   */
  for ( unsigned long i = 0; i < len; i++ ) {
    double angle_t = -msg->angle_min - i * msg->angle_increment;
    if ( type == BACK ) {
      angle_t = angle_t > 0 ? angle_t - Pi : angle_t + Pi;
    }
    if ( msg->ranges[i] < (max_length) ) {
      if ( !t->dots.empty() ) {
        // 距离限制
        if ( abs(t->dots[t->dots.size() - 1].l - msg->ranges[i]) < 0.05 ) {
          t->dots.emplace_back(msg->ranges[i], angle_t);
        }
        else {
          lines.push_back(*t);
          t = new line();
          t->dots.emplace_back(msg->ranges[i], angle_t);
        }
      }
      else {
        t->dots.emplace_back(msg->ranges[i], angle_t);
      }
    }
    else {
      if ( t->dots.empty() ) {
        continue;
      }
      if ( abs(t->dots[t->dots.size() - 1].angle - angle_t) <=
          3 * msg->angle_increment ) {
        continue;
      }
      else {
        lines.push_back(*t);
        t = new line();
      }
    }
  }
  if ( !t->dots.empty() ) {
    lines.push_back(*t);
  }
  if ( lines.empty() ) {
    return 0;
  }
  // 过零处理
  len = lines.size();
  if ( abs(lines[0].dots[0].l -
          lines[len - 1].dots[lines[len - 1].dots.size() - 1].l) < 0.1 &&
      lines[0].dots[0].angle > -1 * Pi / 10 &&
      lines[0].dots[0].angle < Pi / 100 &&
      lines[len - 1].dots[lines[len - 1].dots.size() - 1].angle < Pi / 10 &&
      lines[len - 1].dots[lines[len - 1].dots.size() - 1].angle >
          -1 * Pi / 100 ) {
    t = new line();
    for ( int i = lines[0].dots.size() - 1; i >= 0; i-- ) {
      t->dots.emplace_back(lines[0].dots[i].x, lines[0].dots[i].y, 1);
    }
    for ( int i = lines[len - 1].dots.size() - 1; i >= 0; i-- ) {
      t->dots.emplace_back(lines[len - 1].dots[i].x, lines[len - 1].dots[i].y, 1);
    }
    lines.erase(lines.begin());
    lines.erase(lines.end() - 1);
    lines.push_back(*t);
  }
  //**************************************************************************

  int t1 = 0;

  if ( type == SIDE ) {
    // 这里是侧方停车获取姿态的地方
    t1 = count_side_sign(lines);
  }
  else if ( type == HEAD || type == BACK ) {
    if ( flag == 0 ) {
      t1 = count_two_sign(lines);
    }
    else if ( flag == 1 ) {
      t1 = count_one_sign(lines);
    }
  }
  return t1;
}

inline int path::count_side_sign(vector<line>& lines)
{
  vector<line> t_lines;
  dot          PL, PR;
  // 寻找在范围内的线段并进行填充
  for ( auto& line : lines ) {
    if ( line.dots[0].angle < 0 && line.dots.size() >= 5 ) {
      line.fill_line();
      t_lines.push_back(line);
    }
  }
}

inline int path::count_one_sign(vector<line>& lines)
{
  vector<line> t_lines;
  line         PL;
  // 寻找在范围内的线段并进行填充
  if ( type == HEAD || type == BACK ) {
    for ( auto& line : lines ) {
      if ( line.dots[0].angle > (-Pi / 4) && line.dots[0].angle < (Pi / 4) ) {
        line.fill_line();
        t_lines.push_back(line);
      }
    }
  }
  // 检查t_lines的点数，并进行PR和PL赋值
  if ( t_lines.size() == 1 ) {
    PL = t_lines[0];
  }
  else {
    int    max   = 0;
    double max_y = 0;
    for ( int i = 0; i < t_lines.size(); i++ ) {
      if ( t_lines[i].dot_mid.y > max_y ) {
        max   = i;
        max_y = t_lines[i].dot_mid.y;
      }
    }
    PL = t_lines[max];
  }

  car_cur       = PL.dot_mid;
  car_cur.angle = atan(PL.k);
  if ( type == HEAD && car_cur.l < 0.455 ) {   // 0.42
    flag = 2;
    return 0;
  }
  else if ( type == BACK && car_cur.l < 0.36 ) {
    flag = 2;
    return 0;
  }
  cout << car_cur << endl;
  return 1;
}

inline int path::count_two_sign(vector<line>& lines)
{
  vector<line> t_lines;
  dot          PL, PR;
  // 寻找在范围内的线段并进行填充
  for ( auto& line : lines ) {
    line.fill_line();
  }

  for ( auto& line : lines ) {
    if ( line.dots[0].angle > (-Pi / 4) && line.dots[0].angle < (Pi / 2) &&
        line.dots.size() >= 3 ) {
      if ( abs(line.k) < 1 ) {
        t_lines.push_back(line);
      }
    }
  }
  // 检查t_lines的点数，并进行PR和PL赋值
  if ( t_lines.size() <= 1 ) {
    // 这个真没办法处理，一般情况下都是出错了，直接返回0
    return 0;
  }
  else if ( t_lines.size() == 2 ) {
    if ( abs(t_lines[0].dots[0].y) > abs(t_lines[1].dots[0].y) ) {
      PL = t_lines[0].dot_mid;
      PR = t_lines[1].dot_mid;
    }
    else {
      PL = t_lines[1].dot_mid;
      PR = t_lines[0].dot_mid;
    }
  }
  else {
    sort(t_lines.begin(), t_lines.end());
    // 寻找y距离相差不多的两个线进行处理
    // 基本确定一定能找到，就省去了异常处理
    for ( int i = 0; i < t_lines.size() - 1; i++ ) {
      double y1     = abs(t_lines[i].dot_mid.y) - abs(t_lines[i + 1].dot_mid.y);
      double angle1 = t_lines[i].k - t_lines[i + 1].k;
      if ( abs(y1) < 0.4 && abs(angle1) < 0.2 ) {
        if ( y1 > 0 ) {
          PL = t_lines[i].dot_mid;
          PR = t_lines[i + 1].dot_mid;
        }
        else {
          PL = t_lines[i + 1].dot_mid;
          PR = t_lines[i].dot_mid;
        }
        break;
      }
    }
  }

  // 矩阵求解参数，存储到matrix_canshu矩阵中
  Eigen::Matrix<double, 4, 1> matrix_canshu = mat_count(PL, PR);
  double                      square =
      matrix_canshu(0) * matrix_canshu(0) + matrix_canshu(1) * matrix_canshu(1);

  if ( abs(square - 1) <= 1 ) {
    car_cur.x = matrix_canshu(2);
    car_cur.y = matrix_canshu(3);
    car_cur.fill_dot();
    // 检测停车是否完成
    if ( sqrt((car_cur.x - 0.1) * (car_cur.x - 0.1) + (car_cur.y + 0.21) * (car_cur.y + 0.21)) <
        0.2 ) {
      flag = 1;
      vpoints.clear();
      dvp.clear();
      cout << "第一段停车完成了！！！！！！！！！！！！！！！！！！！！！！！！"
           << endl;
      return 0;
    }

    car_cur.angle = asin(-1 * matrix_canshu(1));

    cout << car_cur << endl;
    cout << "square : " << square << endl;

    return 1;
  }
  else {
    cout << "\033[31msquare is not fit\033[0m" << endl;
  }
  return 0;
}

double path::trace_one_sign()
{
  return -0.55 * car_cur.angle;
}

double path::trace_two_sign()
{
  int    min = 0;
  double e   = 10000;
  double k   = 0.1;   // 1
  for ( int i = 0; i < dvp.size(); i++ ) {
    double dis = sqrt((car_cur.x - vpoints[i].x) * (car_cur.x - vpoints[i].x) + (car_cur.y - vpoints[i].y) * (car_cur.y - vpoints[i].y));
    if ( dis < e ) {
      e   = dis;
      min = i;
    }
  }

  double theta_e;

  if ( atan(dvp[min]) > 0 ) {
    theta_e = (Pi / 2 - atan(dvp[min])) - car_cur.angle;
  }
  else {
    theta_e = (-Pi / 2 - atan(dvp[min])) - car_cur.angle;
  }
  if ( theta_e > 0 )
    return theta_e + atan(k * e);
  else
    return theta_e - atan(k * e);
  // return theta_e + atan(k * e);
}

Eigen::Matrix<double, 4, 1> path::mat_count(dot& PL, dot& PR)
{
  Eigen::Matrix<double, 4, 1> matrix_canshu;
  Eigen::Matrix<double, 4, 4> matrix_car;
  Eigen::Matrix<double, 4, 1> matrix_world;
  if ( type == HEAD ) {
    matrix_world << -0.004, 0.295, 0.579, 0.219;   //-0.026,0.317,0.578,0.262
                                                 //        matrix_world << -0.0544, -0.326, 0.566, 0.197;
  }
  else if ( type == BACK ) {
    matrix_world << -0.041, 0.270, 0.541, 0.272;
  }
  else if ( type == SIDE ) {
    matrix_world << 0, 0, 0, 0;
  }
  matrix_car << PL.x, -PL.y, 1, 0, PR.x, -PR.y, 1, 0, PL.y, PL.x, 0, 1, PR.y,
      PR.x, 0, 1;
  //    matrix_car << PL.x, -PL.y, 1, 0, PR.x, -PR.y, 1, 0, PL.y, PL.x, 0, 1,
  //    PR.y,
  //        PR.x, 0, 1;
  matrix_canshu = matrix_car.inverse() * matrix_world;

  if ( type == HEAD || type == BACK ) {
    matrix_canshu(3) -= 0.21;   // 补偿
  }
  return matrix_canshu;
}

void path::generate_vpoints(int InsertNum)
{
  if ( vpoints.empty() )
    return;
  int Num = vpoints.size();

  int InsertNumSum = 0;   //  计算需要插入的点总数
  for ( int i = 0; i < Num - 1; i++ )
    InsertNumSum += InsertNum;

  //  将折线延长线上两点加入作为首点和尾点
  auto* temp = new dot[Num + 2];
  for ( int i = 0; i < Num; i++ )
    temp[i + 1] = vpoints[i];
  temp[0].x       = 2 * temp[1].x - temp[2].x;
  temp[0].y       = 2 * temp[1].y - temp[2].y;
  temp[Num + 1].x = 2 * temp[Num].x - temp[Num - 1].x;
  temp[Num + 1].y = 2 * temp[Num].y - temp[Num - 1].y;

  dot    NodePt1, NodePt2, NodePt3, NodePt4;
  double t;
  vpoints.resize(Num + InsertNumSum);

  int totalnum = 0;
  for ( int i = 0; i < Num - 1; i++ )   //  每条线段均匀插入点
  {
    NodePt1   = temp[i];
    NodePt2   = temp[i + 1];
    NodePt3   = temp[i + 2];
    NodePt4   = temp[i + 3];
    double dt = 1.0 / (InsertNum + 1);

    for ( int j = 0; j < InsertNum + 1; j++ ) {
      t                   = dt * j;
      vpoints[totalnum].x = F03(t) * NodePt1.x + F13(t) * NodePt2.x +
                            F23(t) * NodePt3.x + F33(t) * NodePt4.x;
      vpoints[totalnum].y = F03(t) * NodePt1.y + F13(t) * NodePt2.y +
                            F23(t) * NodePt3.y + F33(t) * NodePt4.y;
      totalnum++;
    }

    if ( i == Num - 2 ) {   //  最后一个尾点
      t                   = 1;
      vpoints[totalnum].x = F03(t) * NodePt1.x + F13(t) * NodePt2.x +
                            F23(t) * NodePt3.x + F33(t) * NodePt4.x;
      vpoints[totalnum].y = F03(t) * NodePt1.y + F13(t) * NodePt2.y +
                            F23(t) * NodePt3.y + F33(t) * NodePt4.y;
      totalnum++;
    }
  }
  delete[] temp;
}

void path::generate_dvp()
{
  // 生成导数队列
  int Num = vpoints.size();
  dvp.clear();
  dvp.resize(Num);
  for ( int i = 1; i < Num - 1; i++ ) {
    if ( abs(vpoints[i + 1].x - vpoints[i - 1].x) < 0.001 ) {
      dvp[i] = 50000;
    }
    else {
      dvp[i] = (vpoints[i + 1].y - vpoints[i - 1].y) /
               (vpoints[i + 1].x - vpoints[i - 1].x);
    }
  }
  dvp[0]       = dvp[1];
  dvp[Num - 1] = dvp[Num - 2];
}

/**
 * \name LINE
 * \brief
 * 成员函数定义
 */
void line::fill_line()
{
  unsigned long long n = dots.size();

  double sumX = 0.0, sumY = 0.0, sumXY = 0.0, sumX2 = 0.0;
  for ( auto& i : dots ) {
    sumX += i.x;
    sumY += i.y;
    sumXY += i.x * i.y;
    sumX2 += i.x * i.x;
  }

  k = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX);
  b = (sumY - k * sumX) / n;
  double x_mid =
      (b * tan(dots[0].angle) / (1 - k * tan(dots[0].angle)) +
       b * tan(dots[n - 1].angle) / (1 - k * tan(dots[n - 1].angle))) /
      2;

  dot_mid = dot(x_mid, k * x_mid + b, -1);
}
